/* The is the Rover Team Project Code for The California Institute of Technology 2012.
Copyright (C) 2012 scchang, bryanhe, dflicker

This program is free software; you can redistribute it and/or
modify it under the terms of the GNU General Public License
as published by the Free Software Foundation; either version 2
of the License, or (at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA. */

#ifndef COMMAND_
#define COMMAND_

#define PI 3.14159265

#include "include/command.h"

unsigned char one = (unsigned char)128;
unsigned char two = (unsigned char)128;

unsigned char three = (unsigned char)128;
unsigned char four = (unsigned char)128;

Command::Command(InputState* in_state, RoverState* rvr_state){
  bzero(comps, NUM_COMMAND_COMS);
}

Command::Command(RoverState* rvr_state){
  bzero(comps, NUM_COMMAND_COMS);
  // TODO Command logic here
}

int Command::getStringLength(void){
  return (2 * NUM_COMMAND_COMS) + 1;
}

unsigned char Command::getValue(Command_Component comp){
  
  // Sanity Check
  int intValue = (int)comp;
  if(intValue < 0 || intValue >= NUM_COMMAND_COMS){
    fprintf(stderr, "In Command::getValue, invalid Command_Component.\n");
    return (char)128;
  }

  return this->comps[intValue];
}

int Command::setValue(Command_Component comp, char value){

  // Sanity Check
  int intValue = (int)comp;
  if(intValue < 0 || intValue > NUM_COMMAND_COMS){
    fprintf(stderr, "In Command::setValue, invalid Command_Component.\n");
    return -1;
  }

  this->comps[intValue] = value;
  return 0;
}

int Command::toString(unsigned char** buffer){
  // We add two for the inputs
  int size = (2 * NUM_COMMAND_COMS) + 1;
  unsigned char* output = new unsigned char[size];

  // Buffer Zero
  bzero(output, size);

  // Endpoints
  output[0] = STRING_STAR_POINT;
  output[size - 1] = STRING_END_POINT;

  // Add Delinators
  for(int i = 2; i < size - 1; i += 2){
    output[i] = STRING_DELINATOR;
  }

  // Now Actual Information
  int index = 0;
  for(int i = 1; i < size - 1; i += 2, index++){
    output[i] = this->comps[index];
  }

  *buffer = output;

  return 0;
}

Command Command::getCommand(unsigned char* input){
  int size = (2 * NUM_COMMAND_COMS) + 1;

  // Sanity Check
  if(check_correct_delinate(input, size) < 0){
    fprintf(stderr, "In Command::getCommand, Incorrect Delination of Input String.\n");
    RoverState rover = RoverState();
    return Command(&rover);
  }

  // Assuming correct delinaiton
  int index = 0;
  InputState inputState = InputState();
  RoverState roverState = RoverState();
  Command output = Command(&inputState, &roverState);
  for(int i = 1; i < size; i += 2, index++){
    output.comps[index] = input[i];
  }

  return output;
}

void Command::verify(void){
  RoverState rover = RoverState();
  InputState inputState = InputState();
  Command input = Command(&inputState, &rover);
  unsigned char assign[NUM_COMMAND_COMS];
  unsigned char* string_form;

  for(int i = 0; i < NUM_COMMAND_COMS; i++){
    assign[i] = (unsigned char)(i);
    input.setValue((Command_Component)i, assign[i]);
  }

  for(int i = 0; i < NUM_COMMAND_COMS; i++){
    assert(input.getValue((Command_Component)i) == assign[i]);
  }

  input.toString(&string_form);

  Command state = getCommand(string_form);

  for(int i = 0; i < NUM_COMMAND_COMS; i++){
    assert(state.getValue((Command_Component)i) == input.getValue((Command_Component)i));
  }

  delete string_form;
}

#endif
